//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//

#include "lds_lidar.h"

#include <stdio.h>
#include <string.h>
#include <thread>
#include <memory>
#include <sys/time.h>
double LdsLidar::dtime = 500;
struct timeval LdsLidar::time_t;
std::vector<StampedPointCloud> LdsLidar::cloudbuff;
std::deque<StampedPointCloudIntensity> LdsLidar::cloud_Ibuff;
boost::shared_mutex LdsLidar::cloud_mutex;
boost::shared_mutex LdsLidar::imu_mutex;
boost::shared_mutex LdsLidar::org_mutex;
boost::shared_mutex LdsLidar::I_mutex;
std::shared_ptr<Eigen::Vector3d> LdsLidar::macc;
std::shared_ptr<Eigen::Vector3d> LdsLidar::mgyro;
int LdsLidar::SampleNum;
double only_time;
/** Const varible ------------------------------------------------------------------------------- */
/** User add broadcast code here */
static const char *local_broadcast_code_list[] = {
    "000000000000001",
   "3WEDH7600110891",
};

/** For callback use only */
LdsLidar *g_lidars = nullptr;

/** Lds lidar function ---------------------------------------------------------------------------*/
LdsLidar::LdsLidar()
{
  // gettimeofday(&time_t, NULL);
  // only_time = time_t.tv_sec * 1e6 + time_t.tv_usec;
  // cv::FileStorage LoadMycfg_main("../YmlFiles/Dahua.yml", cv::FileStorage::READ);  

  auto_connect_mode_ = true;
  whitelist_count_ = 0;
  is_initialized_ = false;

  lidar_count_ = 0;
  memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));

  memset(lidars_, 0, sizeof(lidars_));
  for (uint32_t i = 0; i < kMaxLidarCount; i++)
  {
    lidars_[i].handle = kMaxLidarCount;
    /** Unallocated state */
    lidars_[i].connect_state = kConnectStateOff;
  }
}

LdsLidar::~LdsLidar()
{
}

int LdsLidar::InitLdsLidar(std::string broadcast_code_strs, double _dtime)
{
  if (is_initialized_)
  {
    printf("LiDAR data source is already inited!\n");
    return -1;
  }
  dtime = _dtime;
  if (!Init())
  {
    Uninit();
    printf("Livox-SDK init fail!\n");
    return -1;
  }

  LivoxSdkVersion _sdkversion;
  GetLivoxSdkVersion(&_sdkversion);
  printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);

  SetBroadcastCallback(LdsLidar::OnDeviceBroadcast);
  SetDeviceStateUpdateCallback(LdsLidar::OnDeviceChange);

  /** Add commandline input broadcast code */
  if (!broadcast_code_strs.empty() && broadcast_code_strs != "auto")
  {
    LdsLidar::AddBroadcastCodeToWhitelist(broadcast_code_strs.c_str());
  }

  /** Add local broadcast code */
  LdsLidar::AddLocalBroadcastCode();

  if (whitelist_count_)
  {
    LdsLidar::DisableAutoConnectMode();
    printf("Disable auto connect mode!\n");

    printf("List all broadcast code in whiltelist:\n");
    for (uint32_t i = 0; i < whitelist_count_; i++)
    {
      printf("%s\n", broadcast_code_whitelist_[i]);
    }
  }
  else
  {
    LdsLidar::EnableAutoConnectMode();
    printf("No broadcast code was added to whitelist, swith to automatic connection mode!\n");
  }

  /** Start livox sdk to receive lidar data */
  if (!Start())
  {
    Uninit();
    printf("Livox-SDK init fail!\n");
    return -1;
  }

  /** Add here, only for callback use */
  if (g_lidars == nullptr)
  {
    g_lidars = this;
  }
  is_initialized_ = true;
  printf("Livox-SDK init success!\n");

  return 0;
}

int LdsLidar::DeInitLdsLidar(void)
{

  if (!is_initialized_)
  {
    printf("LiDAR data source is not exit");
    return -1;
  }

  Uninit();
  printf("Livox SDK Deinit completely!\n");

  return 0;
}

void LdsLidar::LivoxDataToPointCloud(LivoxExtendRawPoint *p_point_data, uint32_t data_num, PcloudI &cloud)
{
  cloud.reset(new CloudI());
  pcl::PointXYZI one;
  one.x = 0.;
  one.y = 0.;
  one.z = 0;
  one.intensity = 0;
  cloud->points.resize(data_num, one);
  // std::cout << "Intensity the data_nume is : " << data_num << std::endl;
  for (size_t i = 0; i < data_num; ++i)
  {
    // if(p_point_data[i].tag & 0x00000011 == 0x0000001 || p_point_data[i].tag & 0x00001100 == 0x00001000)
    //   continue;
    // std::cout << p_point_data[i].tag << std::endl;

    PointI temp;
    temp.x = p_point_data[i].x / 1000.;
    temp.y = p_point_data[i].y / 1000.;
    temp.z = p_point_data[i].z / 1000.;
    temp.intensity = p_point_data[i].reflectivity;
    
    // temp.intensity = rand() % 50;
    // std::cout << "the intensity is: " << (float)p_point_data[i].reflectivity << "and" << temp.intensity << std::endl;
    cloud->points.push_back(temp);
  }
  cloud->height = 1;
  cloud->width = cloud->points.size();
  cloud->is_dense = false;
}

/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 原始数据转化为点云
 * @param p_point_data 原始数据
 *@param data_num 点的个数
 *@param cloud 点云单位:m
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
void LdsLidar::LivoxDataToPointCloud(LivoxRawPoint *p_point_data, uint32_t data_num, Pcloud &cloud)
{
  cloud.reset(new Cloud);
  cloud->points.resize(data_num, Point(0, 0, 0));
  //     #if _OPENMP///openmp加速
  // #pragma omp parallel for num_threads(1)
  // #endif
  for (int i = 0; i < data_num; ++i)
  {
    Point temp = Point(p_point_data[i].x / 1000., p_point_data[i].y / 1000., p_point_data[i].z / 1000.);
    cloud->points[i] = temp;
  }
  cloud->height = 1;
  cloud->width = cloud->points.size();
  cloud->is_dense = false;
}
/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 原始数据转化为点云
 * @param p_point_data 原始数据
 *@param data_num 点的个数
 *@param cloud 点云单位:m
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
void LdsLidar::LivoxDataToPointCloud(LivoxSpherPoint *p_point_data, uint32_t data_num, Pcloud &cloud)
{
  cloud.reset(new Cloud);
  cloud->points.resize(data_num, Point(0, 0, 0));
  //     #if _OPENMP///openmp加速
  // #pragma omp parallel for num_threads(1)
  // #endif
  for (int i = 0; i < data_num; ++i)
  {
    Point temp;
    temp.x = p_point_data->depth * sin(p_point_data->theta) * cos(p_point_data->phi) / 1000.;
    temp.y = p_point_data->depth * sin(p_point_data->phi) * sin(p_point_data->theta) / 1000.;
    temp.z = p_point_data->depth * cos(p_point_data->theta) / 1000.;
    cloud->points[i] = temp;
  }
  cloud->height = 1;
  cloud->width = cloud->points.size();
  cloud->is_dense = false;
}
/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 原始数据转化为点云
 * @param p_point_data 原始数据
 *@param data_num 点的个数
 *@param cloud 点云单位:m
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
void LdsLidar::LivoxDataToPointCloud(LivoxExtendRawPoint *p_point_data, uint32_t data_num, Pcloud &cloud)
{
  cloud.reset(new Cloud);
  cloud->points.resize(data_num, Point(0, 0, 0));
  // std::cout << "ExtendRawPoint the data_num is : " << data_num << std::endl; 
  //     #if _OPENMP///openmp加速
  // #pragma omp parallel for num_threads(1)
  // #endif
  for (int i = 0; i < data_num; ++i)
  {
    Point temp = Point(p_point_data[i].x / 1000., p_point_data[i].y / 1000., p_point_data[i].z / 1000.);
    cloud->points[i] = temp;
  }
  cloud->height = 1;
  cloud->width = cloud->points.size();
  cloud->is_dense = false;
}
/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 原始数据转化为点云
 * @param p_point_data 原始数据
 *@param data_num 点的个数
 *@param cloud 点云单位:m
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
void LdsLidar::LivoxDataToPointCloud(LivoxExtendSpherPoint *p_point_data, uint32_t data_num, Pcloud &cloud)
{
  cloud.reset(new Cloud);
  cloud->points.resize(data_num, Point(0, 0, 0));
  //     #if _OPENMP///openmp加速
  // #pragma omp parallel for num_threads(1)
  // #endif
  for (int i = 0; i < data_num; ++i)
  {
    Point temp;
    temp.x = p_point_data->depth * sin(p_point_data->theta) * cos(p_point_data->phi) / 1000.;
    temp.y = p_point_data->depth * sin(p_point_data->phi) * sin(p_point_data->theta) / 1000.;
    temp.z = p_point_data->depth * cos(p_point_data->theta) / 1000.;
    cloud->points[i] = temp;
  }
  cloud->height = 1;
  cloud->width = cloud->points.size();
  cloud->is_dense = false;
}
/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 原始数据转化为点云
 * @param p_point_data 原始数据
 *@param data_num 点的个数
 *@param cloud 点云单位:m
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
void LdsLidar::LivoxDataToPointCloud(LivoxDualExtendRawPoint *p_point_data, uint32_t data_num, Pcloud &cloud)
{
  cloud.reset(new Cloud);
  cloud->points.resize(data_num, Point(0, 0, 0));
  //     #if _OPENMP///openmp加速
  // #pragma omp parallel for num_threads(1)
  // #endif
  for (int i = 0; i < data_num; ++i)
  {
    Point temp = Point(p_point_data[i].x1 / 1000., p_point_data[i].y1 / 1000., p_point_data[i].z1 / 1000.);
    cloud->points[i] = temp;
  }
  cloud->height = 1;
  cloud->width = cloud->points.size();
  cloud->is_dense = false;
}
/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 原始数据转化为点云
 * @param p_point_data 原始数据
 *@param data_num 点的个数
 *@param cloud 点云 单位:m
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
void LdsLidar::LivoxDataToPointCloud(LivoxDualExtendSpherPoint *p_point_data, uint32_t data_num, Pcloud &cloud)
{
  cloud.reset(new Cloud);
  cloud->points.resize(data_num, Point(0, 0, 0));
  //     #if _OPENMP///openmp加速
  // #pragma omp parallel for num_threads(1)
  // #endif
  for (int i = 0; i < data_num; ++i)
  {
    Point temp;
    temp.x = p_point_data->depth1 * sin(p_point_data->theta) * cos(p_point_data->phi) / 1000.;
    temp.y = p_point_data->depth1 * sin(p_point_data->phi) * sin(p_point_data->theta) / 1000.;
    temp.z = p_point_data->depth1 * cos(p_point_data->theta) / 1000.;
    cloud->points[i] = temp;
  }
  cloud->height = 1;
  cloud->width = cloud->points.size();
  cloud->is_dense = false;
}
/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 原始imu数据转化为Eigen vector
 * @param p_point_data 原始数据
 *@param gyro 角速度 单位g
 *@param acc 加速度 单位rad/s
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
void LdsLidar::LivoxDataToPointCloud(LivoxImuPoint *p_point_data, uint32_t data_num, std::shared_ptr<Eigen::Vector3d> &gyro, std::shared_ptr<Eigen::Vector3d> &acc)
{
  gyro.reset(new Eigen::Vector3d(p_point_data->gyro_x, p_point_data->gyro_y, p_point_data->gyro_z));
  acc.reset(new Eigen::Vector3d(p_point_data->acc_x, p_point_data->acc_y, p_point_data->acc_z));
}

/** Static function in LdsLidar for callback or event process ------------------------------------*/

/** Receiving point cloud data from Livox LiDAR. */
void LdsLidar::GetLidarDataCb(uint8_t handle, LivoxEthPacket *data,
                              uint32_t data_num, void *client_data)
{
  // gettimeofday(&time_t, NULL);
  // static double sample_time = time_t.tv_sec * 1e6 + time_t.tv_usec;
  // std::cout << sample_time - only_time << std::endl;
  // only_time = sample_time;
  // std::chrono::time_point<std::chrono::nanoseconds> now_time = std::chrono::time_point_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now());
  // std::cout << now_time.time_since_epoch().count() << std::endl;
  // clock_t curr_time = std::clock();
  // std::cout << curr_time*1.0 / CLOCKS_PER_SEC << std::endl;
  // return;
  using namespace std;
  Pcloud cloud_tp;
  PcloudI cloud_tpI;
  LdsLidar *lidar_this = static_cast<LdsLidar *>(client_data);
  LivoxEthPacket *eth_packet = data;

  if (!data || !data_num || (handle >= kMaxLidarCount))
  {
    return;
  }

  if (eth_packet)
  {
    lidar_this->data_recveive_count_[handle]++;
    // printf("receive packet count %d %d\n", handle, lidar_this->data_recveive_count_[handle]);
    /** Parsing the timestamp and the point cloud data. */
    uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
    if (data->data_type == kCartesian)
    {
      // std::cerr<<"data type:"<<1<<std::endl;
      LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
      LivoxDataToPointCloud(p_point_data, data_num, cloud_tp);
      // LivoxDataToPointCloud(p_point_data, data_num, cloud_tpI);
      std::cout << "kCartesian" << std::endl;
    }
    else if (data->data_type == kSpherical)
    {
      // std::cerr<<"data type:"<<2<<std::endl;
      LivoxSpherPoint *p_point_data = (LivoxSpherPoint *)data->data;
      LivoxDataToPointCloud(p_point_data, data_num, cloud_tp);
    }
    else if (data->data_type == kExtendCartesian)
    {
      // std::cerr<<"data type:"<<3<<std::endl;
      LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *)data->data;
      // std::cout << "kExtendCartesian" << std::endl;
      LivoxDataToPointCloud(p_point_data, data_num, cloud_tpI);
      LivoxDataToPointCloud(p_point_data, data_num, cloud_tp);
    }
    else if (data->data_type == kExtendSpherical)
    {
      // std::cerr<<"data type:"<<4<<std::endl;
      LivoxExtendSpherPoint *p_point_data = (LivoxExtendSpherPoint *)data->data;
      LivoxDataToPointCloud(p_point_data, data_num, cloud_tp);
    }
    else if (data->data_type == kDualExtendCartesian)
    {
      // std::cerr<<"data type:"<<5<<std::endl;
      LivoxDualExtendRawPoint *p_point_data = (LivoxDualExtendRawPoint *)data->data;
      LivoxDataToPointCloud(p_point_data, data_num, cloud_tp);
    }
    else if (data->data_type == kDualExtendSpherical)
    {
      // std::cerr<<"data type:"<<6<<std::endl;
      LivoxDualExtendSpherPoint *p_point_data = (LivoxDualExtendSpherPoint *)data->data;
      LivoxDataToPointCloud(p_point_data, data_num, cloud_tp);
    }
    else if (data->data_type == kImu)
    {
      // std::cerr<<"data type:"<<7<<std::endl;
      LivoxImuPoint *p_point_data = (LivoxImuPoint *)data->data;
      std::shared_ptr<Eigen::Vector3d> gyro_tp, acc_tp;
      LivoxDataToPointCloud(p_point_data, data_num, gyro_tp, acc_tp);
      if (gyro_tp && acc_tp)
      {
        boost::unique_lock<boost::shared_mutex> lock(imu_mutex);
        mgyro = gyro_tp;
        macc = acc_tp;
      }
    }

    if (cloud_tp)
    { //这个为一般点云赋值(无intensity参数的点云)
      gettimeofday(&time_t, NULL);
      static int cnt = 0;
      // double now_time = time_t.tv_sec * 1e3 + time_t.tv_usec * 1e-3;
      double start_time, end_time;
      if(cnt == 0)
      {
        start_time = time_t.tv_sec*1e3+time_t.tv_usec*1e-3;
      }
      //每个采样间隔输出一次采样时间
      if(cnt % 400== 399)
      {
        end_time = time_t.tv_sec*1e3+time_t.tv_usec*1e-3;
        std::cout << "duration:" << end_time - start_time << std::endl;
        start_time = end_time;
      }

      StampedPointCloud scloud;
      scloud.cloud = cloud_tp;
      // scloud.time = now_time;
      org_mutex.lock();
      // if(!cloudbuff.empty())
      //   {
      //     std::cout << "the time gap: " << now_time - cloudbuff.back().time << std::endl;
      //     std::cout << "every package of the cloud is: " << cloudbuff.back().cloud->size() << std::endl;
      //   }
      
      //将每个数据包存入cloudbuff中
      cloudbuff.emplace_back(scloud);
      org_mutex.unlock();
      cnt++;
      // while (now_time - cloudbuff.front().time > dtime)
      // {
      //   org_mutex.lock();
      //   cloudbuff.pop_front();
      //   org_mutex.unlock();
      // }
      //当数据包大于250个时，会把旧的全部剔除
      if(cnt >= 400)
      {
        org_mutex.lock();
        // cloudbuff.pop_front();
        cloudbuff.erase(cloudbuff.begin());
        org_mutex.unlock();
      }
    }

    if (cloud_tpI)
    {
      gettimeofday(&time_t, NULL);
      double now_time = time_t.tv_sec * 1e3 + time_t.tv_usec * 1e-3;
      StampedPointCloudIntensity scloudI;
      scloudI.cloud = cloud_tpI;
      scloudI.time = now_time;
      I_mutex.lock();
      cloud_Ibuff.emplace_back(scloudI);
      I_mutex.unlock();
      while (now_time - cloud_Ibuff.front().time > dtime)
      {
        I_mutex.lock();
        cloud_Ibuff.pop_front();
        I_mutex.unlock();
      }
    }
  }
}
/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 获取原始,可以用来去除运动畸变
 * @return 原始点云
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
std::vector<StampedPointCloud> LdsLidar::getOriginCloud()
{
  org_mutex.lock();
  auto temp = cloudbuff;
  org_mutex.unlock();
  return temp;
}

std::deque<StampedPointCloudIntensity> LdsLidar::getOriginCloud_I()
{
  I_mutex.lock();
  auto temp = cloud_Ibuff;
  I_mutex.unlock();
  return temp;
}

PcloudI LdsLidar::GetPointCloud_I(size_t num_thread)
{
  I_mutex.lock();
  auto temp = cloud_Ibuff;
  I_mutex.unlock();
  if (temp.size() < 1)
    return NULL;

  size_t accnum[temp.size() + 1];
  accnum[0] = 0;
  for (size_t i = 1; i < temp.size() + 1; i++)
  {
    accnum[i] = accnum[i - 1] + temp[i - 1].cloud->points.size();
  }
  PcloudI mcloud;
  mcloud.reset(new CloudI);
  mcloud->points.resize(accnum[temp.size()]);
#if _OPENMP /// openmp加速
#pragma omp parallel for num_threads(num_thread)
#endif
  for (size_t i = 0; i < temp.size(); ++i)
  {
    for (int j = 0; j < temp[i].cloud->points.size(); ++j)
    {
      mcloud->points[accnum[i] + j] = temp[i].cloud->points[j];
    }
    // mcloud->points.insert(mcloud->points.begin()+accnum[i],temp[i].cloud->points.begin(),temp[i].cloud->points.end());
  }
  return mcloud;
}

/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 获取点云
 * @param num_thread:omp线程数不能太高可以试试1或者2
 * @return 点云,为NULL时表示获取失败,单位:m
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
Pcloud LdsLidar::GetPointCloud(size_t num_thread)
{
  org_mutex.lock();
  auto temp = cloudbuff;
  org_mutex.unlock();
  if (temp.size() < 1)
  {
    return NULL;
  }            
  size_t accnum[temp.size() + 1];
  accnum[0] = 0;
  for (size_t i = 1; i < temp.size() + 1; ++i)
  {
    accnum[i] = accnum[i - 1] + temp[i - 1].cloud->points.size();
  }
  Pcloud mcloud;
  mcloud.reset(new Cloud);
  mcloud->points.resize(accnum[temp.size()]);
#if _OPENMP /// openmp加速
#pragma omp parallel for num_threads(num_thread)
#endif
  for (size_t i = 0; i < temp.size(); ++i)
  {
    for (int j = 0; j < temp[i].cloud->points.size(); ++j)
    {
      mcloud->points[accnum[i] + j] = temp[i].cloud->points[j];
    }
    // mcloud->points.insert(mcloud->points.begin()+accnum[i],temp[i].cloud->points.begin(),temp[i].cloud->points.end());
  }
  return mcloud;
}
/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 获取角速度
 * @param destroy 设为true避免重复使用同一帧数据
 * @return 角速度单位:rad/s
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
std::shared_ptr<Eigen::Vector3d> LdsLidar::GetGyro(bool destroy)
{
  if (destroy)
  {
    boost::unique_lock<boost::shared_mutex> lock(imu_mutex);
    auto temp = mgyro;
    mgyro.reset();
    return temp;
  }
  else
  {
    boost::shared_lock<boost::shared_mutex> lock(cloud_mutex);
    return mgyro;
  }
}
/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 获取加速度
 * @param destroy 设为true避免重复使用同一帧数据
 * @return 加速度,单位:g
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
std::shared_ptr<Eigen::Vector3d> LdsLidar::GetAcc(bool destroy)
{
  if (destroy)
  {
    boost::unique_lock<boost::shared_mutex> lock(imu_mutex);
    auto temp = macc;
    macc.reset();
    return temp;
  }
  else
  {
    boost::shared_lock<boost::shared_mutex> lock(cloud_mutex);
    return macc;
  }
}
/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 获取加速度与角速度
 * @param destroy 设为true避免重复使用同一帧数据
 * @param gyro 角速度,单位:rad/s
 * @param acc 加速度,单位:g
 * @return
 * @author 黎林
 * -phone:18846140245;qq:1430244438
 */
void LdsLidar::GetImuData(std::shared_ptr<Eigen::Vector3d> &gyro, std::shared_ptr<Eigen::Vector3d> &acc, bool destroy)
{
  if (destroy)
  {
    boost::unique_lock<boost::shared_mutex> lock(imu_mutex);
    auto temp1 = mgyro;
    auto temp2 = macc;
    mgyro.reset();
    macc.reset();
    gyro = temp1;
    acc = temp2;
  }
  else
  {
    boost::shared_lock<boost::shared_mutex> lock(cloud_mutex);
    gyro = mgyro;
    acc = macc;
  }
}

void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
{
  if (info == nullptr)
  {
    return;
  }

  if (info->dev_type == kDeviceTypeHub)
  {
    printf("In lidar mode, couldn't connect a hub : %s\n", info->broadcast_code);
    return;
  }

  if (g_lidars->IsAutoConnectMode())
  {
    printf("In automatic connection mode, will connect %s\n", info->broadcast_code);
  }
  else
  {
    if (!g_lidars->FindInWhitelist(info->broadcast_code))
    {
      printf("Not in the whitelist, please add %s to if want to connect!\n",
             info->broadcast_code);
      return;
    }
  }

  bool result = false;
  uint8_t handle = 0;
  result = AddLidarToConnect(info->broadcast_code, &handle);
  if (result == kStatusSuccess && handle < kMaxLidarCount)
  {
    SetDataCallback(handle, LdsLidar::GetLidarDataCb, (void *)g_lidars);
    LidarDevice *p_lidar = &(g_lidars->lidars_[handle]);
    p_lidar->handle = handle;
    p_lidar->connect_state = kConnectStateOff;
    p_lidar->config.enable_fan = true;
    // p_lidar->config.return_mode = kStrongestReturn;  //设置回波类型的参数！！！
    p_lidar->config.return_mode = kDualReturn;
    p_lidar->config.coordinate = kCoordinateCartesian;
    p_lidar->config.imu_rate = kImuFreq200Hz;
  }
  else
  {
    printf("Add lidar to connect is failed : %d %d \n", result, handle);
  }
}

/** Callback function of changing of device state. */
void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type)
{
  if (info == nullptr)
  {
    return;
  }

  uint8_t handle = info->handle;
  if (handle >= kMaxLidarCount)
  {
    return;
  }

  LidarDevice *p_lidar = &(g_lidars->lidars_[handle]);
  if (type == kEventConnect)
  {
    QueryDeviceInformation(handle, DeviceInformationCb, g_lidars);
    if (p_lidar->connect_state == kConnectStateOff)
    {
      p_lidar->connect_state = kConnectStateOn;
      p_lidar->info = *info;
    }
    printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
  }
  else if (type == kEventDisconnect)
  {
    p_lidar->connect_state = kConnectStateOff;
    printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
  }
  else if (type == kEventStateChange)
  {
    p_lidar->info = *info;
    printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
  }

  if (p_lidar->connect_state == kConnectStateOn)
  {
    printf("Device Working State %d\n", p_lidar->info.state);
    if (p_lidar->info.state == kLidarStateInit)
    {
      printf("Device State Change Progress %u\n", p_lidar->info.status.progress);
    }
    else
    {
      printf("Device State Error Code 0X%08x\n", p_lidar->info.status.status_code.error_code);
    }
    printf("Device feature %d\n", p_lidar->info.feature);
    SetErrorMessageCallback(handle, LdsLidar::LidarErrorStatusCb);

    /** Config lidar parameter */
    if (p_lidar->info.state == kLidarStateNormal)
    {
      if (p_lidar->config.coordinate != 0)
      {
        SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, g_lidars);
      }
      else
      {
        SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, g_lidars);
      }
      p_lidar->config.set_bits |= kConfigCoordinate;

      if (kDeviceTypeLidarMid40 != info->type)
      {
        LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
                                     LdsLidar::SetPointCloudReturnModeCb, g_lidars);
        p_lidar->config.set_bits |= kConfigReturnMode;
      }

      if (kDeviceTypeLidarMid40 != info->type)
      {
        LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
                                 LdsLidar::SetImuRatePushFrequencyCb, g_lidars);
        p_lidar->config.set_bits |= kConfigImuRate;
      }

      p_lidar->connect_state = kConnectStateConfig;
    }
  }
}

/** Query the firmware version of Livox LiDAR. */
void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle,
                                   DeviceInformationResponse *ack, void *client_data)
{
  if (status != kStatusSuccess)
  {
    printf("Device Query Informations Failed : %d\n", status);
  }
  if (ack)
  {
    printf("firm ver: %d.%d.%d.%d\n",
           ack->firmware_version[0],
           ack->firmware_version[1],
           ack->firmware_version[2],
           ack->firmware_version[3]);
  }
}

/** Callback function of Lidar error message. */
void LdsLidar::LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message)
{
  static uint32_t error_message_count = 0;
  if (message != NULL)
  {
    ++error_message_count;
    if (0 == (error_message_count % 100))
    {
      printf("handle: %u\n", handle);
      printf("temp_status : %u\n", message->lidar_error_code.temp_status);
      printf("volt_status : %u\n", message->lidar_error_code.volt_status);
      printf("motor_status : %u\n", message->lidar_error_code.motor_status);
      printf("dirty_warn : %u\n", message->lidar_error_code.dirty_warn);
      printf("firmware_err : %u\n", message->lidar_error_code.firmware_err);
      printf("pps_status : %u\n", message->lidar_error_code.device_status);
      printf("fan_status : %u\n", message->lidar_error_code.fan_status);
      printf("self_heating : %u\n", message->lidar_error_code.self_heating);
      printf("ptp_status : %u\n", message->lidar_error_code.ptp_status);
      printf("time_sync_status : %u\n", message->lidar_error_code.time_sync_status);
      printf("system_status : %u\n", message->lidar_error_code.system_status);
    }
  }
}

void LdsLidar::ControlFanCb(livox_status status, uint8_t handle,
                            uint8_t response, void *client_data)
{
}

void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle,
                                         uint8_t response, void *client_data)
{
  LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);

  if (handle >= kMaxLidarCount)
  {
    return;
  }
  LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);

  if (status == kStatusSuccess)
  {
    p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
    printf("Set return mode success!\n");

    if (!p_lidar->config.set_bits)
    {
      LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
      p_lidar->connect_state = kConnectStateSampling;
    }
  }
  else
  {
    LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
                                 LdsLidar::SetPointCloudReturnModeCb, lds_lidar);
    printf("Set return mode fail, try again!\n");
  }
}

void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle,
                               uint8_t response, void *client_data)
{
  LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);

  if (handle >= kMaxLidarCount)
  {
    return;
  }
  LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);

  if (status == kStatusSuccess)
  {
    p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
    printf("Set coordinate success!\n");

    if (!p_lidar->config.set_bits)
    {
      LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
      p_lidar->connect_state = kConnectStateSampling;
    }
  }
  else
  {
    if (p_lidar->config.coordinate != 0)
    {
      SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
    }
    else
    {
      SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
    }

    printf("Set coordinate fail, try again!\n");
  }
}

void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
                                         uint8_t response, void *client_data)
{
  LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);

  if (handle >= kMaxLidarCount)
  {
    return;
  }
  LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);

  if (status == kStatusSuccess)
  {
    p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
    printf("Set imu rate success!\n");

    if (!p_lidar->config.set_bits)
    {
      LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
      p_lidar->connect_state = kConnectStateSampling;
    }
  }
  else
  {
    LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
                             LdsLidar::SetImuRatePushFrequencyCb, lds_lidar);
    printf("Set imu rate fail, try again!\n");
  }
}

/** Callback function of starting sampling. */
void LdsLidar::StartSampleCb(livox_status status, uint8_t handle,
                             uint8_t response, void *client_data)
{
  LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);

  if (handle >= kMaxLidarCount)
  {
    return;
  }

  LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
  if (status == kStatusSuccess)
  {
    if (response != 0)
    {
      p_lidar->connect_state = kConnectStateOn;
      printf("Lidar start sample fail : state[%d] handle[%d] res[%d]\n",
             status, handle, response);
    }
    else
    {
      printf("Lidar start sample success\n");
    }
  }
  else if (status == kStatusTimeout)
  {
    p_lidar->connect_state = kConnectStateOn;
    printf("Lidar start sample timeout : state[%d] handle[%d] res[%d]\n",
           status, handle, response);
  }
}

/** Callback function of stopping sampling. */
void LdsLidar::StopSampleCb(livox_status status, uint8_t handle,
                            uint8_t response, void *client_data)
{
}

/** Add broadcast code to whitelist */
int LdsLidar::AddBroadcastCodeToWhitelist(const char *bd_code)
{
  if (!bd_code || (strlen(bd_code) > kBroadcastCodeSize) ||
      (whitelist_count_ >= kMaxLidarCount))
  {
    return -1;
  }

  if (LdsLidar::FindInWhitelist(bd_code))
  {
    printf("%s is alrealy exist!\n", bd_code);
    return -1;
  }

  strcpy(broadcast_code_whitelist_[whitelist_count_], bd_code);
  ++whitelist_count_;

  return 0;
}

void LdsLidar::AddLocalBroadcastCode(void)
{
  for (size_t i = 0; i < sizeof(local_broadcast_code_list) / sizeof(intptr_t); ++i)
  {
    std::string invalid_bd = "000000000";
    printf("Local broadcast code : %s\n", local_broadcast_code_list[i]);
    if ((kBroadcastCodeSize == strlen(local_broadcast_code_list[i])) &&
        (nullptr == strstr(local_broadcast_code_list[i], invalid_bd.c_str())))
    {
      LdsLidar::AddBroadcastCodeToWhitelist(local_broadcast_code_list[i]);
    }
    else
    {
      printf("Invalid local broadcast code : %s\n", local_broadcast_code_list[i]);
    }
  }
}

bool LdsLidar::FindInWhitelist(const char *bd_code)
{
  if (!bd_code)
  {
    return false;
  }

  for (uint32_t i = 0; i < whitelist_count_; i++)
  {
    if (strncmp(bd_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0)
    {
      return true;
    }
  }

  return false;
}
